/*
 * @FilePath: rx_ops9_node.cpp
 * @Author: wyx
 * @Date: 2023-09-30 19:43:28
 * @LastEditors: Please set LastEditors
 * @LastEditTime: 2023-10-24 10:44:39
 * Copyright: 2023 xxxTech CO.,LTD. All Rights Reserved.
 * @Descripttion: 
 */

#include"rx_ops9/rx_ops9_node.hpp"
using namespace std;
using namespace std::chrono_literals;


std::ofstream OPS_original;
int PPS_handle;
ppsCommSerial pps;

/**
 * 定位系统数据总公是12个浮点数（float），每一个数据4位，依次是：
 * x轴坐标，x速度，y轴坐标，y速度，俯仰角，横滚角，偏航角，俯仰角速度，偏航角速度，x， y, z轴加速度
 *    0     1      2      3     4      5     6        7         8     9  10   11
 * 
 * 帧头AT 帧尾ta
 */
class RxOpsNode :public rclcpp::Node 
{

public:  
    RxOpsNode() : Node("ops9_node") //构造函数
    {
        ops_pub_ = this->create_publisher<base_interface::msg::Ops>("/ops_data", 10);
    

    // 创建输出文件
	time_t currentTime;
	time(&currentTime);
	currentTime = currentTime + 8 * 3600; //	格林尼治标准时间+8个小时
	tm *t = gmtime(&currentTime);	

    string filename_plantime = "data/rx_ops9_txt/OPS_original" + to_string(t->tm_mon + 1) + "-" +to_string(t->tm_mday) + "-" + to_string(t->tm_hour) + "-" + to_string(t->tm_min) + ".txt";
    OPS_original.open(filename_plantime.c_str());
    OPS_original.precision(5);//将输出的精度设置为 5，即将显示小数点后五位数字
    OPS_original.setf(ios_base::showpoint);//令输出流在显示浮点数时始终显示小数点及尾随的零
  
    }

    void Rx_OpsCallback() 
    {


        auto msg = base_interface::msg::Ops();
        int pps_len;
        int count = 0;
        int dataCnt = 0;
        char ch;

        union
        {  
            char dataC[RX_PPS_DATA_LEN];
            float dataF[RX_PPS_DATA_LEN / 4];
        } ppsData;

        while (PPS_handle) 
        {

            pps_len = read(PPS_handle, &ch, 1);
            if (pps_len > 0) 
            {
                switch (count)
                {
                    case 0:
                        if (ch == 'A') 
                            count++;
                        else    
                            count = 0;
                        break;
                    case 1:
                        if (ch == 'A');
                        else if (ch == 'T') 
                        {
                            dataCnt = 0;
                            count++;
                        }
                        else    
                            count = 0;

                        break;
                    case 2:
                        ppsData.dataC[dataCnt] = ch;
                        dataCnt++;
                        if (dataCnt >= RX_PPS_DATA_LEN-4) 
                        {
                            dataCnt = 0;
                            count++;
                        }
                        break;
                    case 3:
                        if (ch == 't') 
                            count++;
                        else    
                            count = 0;
                        break;
                    case 4:
                        if (ch == 'a') 
                        {
                            // msg.x = (ppsData.dataF[0] + DistancetoCenter * sin(msg.theta)) / 1000.0f;
                            // msg.y = (ppsData.dataF[2] - DistancetoCenter * cos(msg.theta) + DistancetoCenter) / 1000.0f;/// 1000.0f
                            // msg.theta = ppsData.dataF[6] * 3.1415926 / 180.f;
                            msg.x     = (float)((int)(ppsData.dataF[0]/ 1000.0f * 10000))/10000;
                            msg.x_vel = (float)((int)(ppsData.dataF[1]/ 1000.0f * 10000))/10000;
                            msg.y     = (float)((int)(ppsData.dataF[2]/ 1000.0f * 10000))/10000;
                            msg.y_vel = (float)((int)(ppsData.dataF[3]/ 1000.0f * 10000))/10000;
                            // msg.x_acc = NumDecimal(-ppsData.dataF[9] * 9.8, 4);
                            // msg.y_acc = NumDecimal( ppsData.dataF[10]* 9.8, 4);
                            msg.x_acc =(float)(-(int)(ppsData.dataF[9] * 9.8 * 10000))/10000;
                            msg.y_acc =(float)( (int)(ppsData.dataF[10]* 9.8 * 10000))/10000;
                            // msg.x_acc = -ppsData.dataF[9] * 9.8;
                            // msg.y_acc =  ppsData.dataF[10]* 9.8;

                            // msg.pitch = ppsData.dataF[4];
                            // msg.roll  = ppsData.dataF[5];
                            
                            msg.theta = (float)((int)(ppsData.dataF[6]* 3.1415926 / 180.f * 10000))/10000 ;



                            msg.time_stamp.stamp = this->now();

                            RCLCPP_INFO(this->get_logger(),"x=%.4f, y=%.4f, x_vel=%.4f,y_vel=%.4f,x_acc = %.4f, y_acc = %.4f,yaw=%.4f", msg.x, msg.y, msg.x_vel, msg.y_vel, msg.x_acc, msg.y_acc, msg.theta);

                            ops_pub_->publish(msg);//发布定位系统消息
                        }
                        count = 0;
                        break;

                    default:
                        count = 0;
                        break;
                }
            }
            
        }     
    }

private:

    rclcpp::Publisher<base_interface::msg::Ops>::SharedPtr ops_pub_;

};



/**
 * @brief: Ops9定位初始化
 * @param {int&} fd
 * @param {char*} port 串口的路径
 * @param {int} baudrate
 * @return {*}
 */
int ppsCommSerial::pps_Init(int& fd, char* port, int baudrate)
{
    std::cout << "init " << port << " ...\n";
    fd = open(port, O_RDWR | O_NOCTTY | O_NDELAY);
    std::cout << fd << std::endl;
    if (fd == -1)
    {
        perror("open_port: Unable to open\n");
        return 0;
    }
    tcgetattr(fd, &termAttr);
    bzero(&termAttr, sizeof(termAttr));
    cfsetispeed(&termAttr, baudrate);
    cfsetospeed(&termAttr, baudrate);
    termAttr.c_cflag &= ~PARENB;
    termAttr.c_cflag &= ~CSTOPB;
    termAttr.c_cflag &= ~CSIZE;
    termAttr.c_cflag |= CS8;
    termAttr.c_cflag |= (CLOCAL | CREAD);
    termAttr.c_lflag &= ~(ICANON | ECHO | ECHOE | ISIG);
    termAttr.c_iflag &= ~(IXON | IXOFF | IXANY | ICRNL | ISTRIP);
    termAttr.c_oflag &= ~OPOST;
    termAttr.c_cc[VMIN] = 1;
    termAttr.c_cc[VTIME] = 0;
    tcsetattr(fd, TCSANOW, &termAttr);
    tcflush(fd, TCIOFLUSH);
    std::cout << port << ": serial init ok!" << endl;
    return fd;
}


/** 
 * @brief 重启定位系统,数据清零
 * @param fd 发送时使用的串口号
 */
void ppsCommSerial::RestartPos(int fd) 
{
    char tdata[16] = {'A', 'Z','e','r','o','D','r','i','f','t','F','l','a','g', '\r', '\n'};
    write(fd, tdata, 16);
    char ch;
    while(read(fd,&ch,1) != -1);
}


int main(int argc, char** argv) 
{
    // 1.初始化ros2
    rclcpp::init(argc, argv);
    std::cout << "pps embedded init .." << std::endl;
    PPS_handle = pps.pps_Init(PPS_handle, "/dev/pps_uart", B115200); // TODO KERNEL=="ttyUSB*", ATTRS{idVendor}=="1a86", ATTRS{idProduct}=="7523", GROUP="users", MODE="0777",SYMLINK+="pps_uart"
    //重启定位系统函数
    // pps.RestartPos(PPS_handle);
    std::cout << "port init and reset done." << std::endl;


    auto opsNode = std::make_shared<RxOpsNode>();
    
    while (rclcpp::ok()) 
    {

        opsNode->Rx_OpsCallback();
        // cout<<"NumDecimal:"<<NumDecimal(1.23456,3)<<endl;
        // RCLCPP_INFO(opsNode->get_logger(),"8888888888888888888888");
        // if(!rclcpp::ok()){
        //     return false;

        // }   
        // rclcpp::sleep_for(std::chrono::seconds(2));

    }
    // 4.调用spin函数,传入自定义对象指针
    // rclcpp::spin(opsNode);
    // 5.释放资源
    rclcpp::shutdown();
    return 0;
}